/*
	Copyright 2008 Jay Elliott

	This file is part of libemath.

	libemath is free software: you can redistribute it and/or modify
	it under the terms of the GNU General Public License as published by
	the Free Software Foundation, either version 3 of the License, or
	(at your option) any later version.

	libemath is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU General Public License for more details.

	You should have received a copy of the GNU General Public License
	along with libemath.  If not, see <http://www.gnu.org/licenses/>.
*/


#include "cOrientedBox.h"


void emath::cOrientedBox::GetPoints(emath::cPoint3d PointBuffer[8], const cPoint3d& extent)
{
	//bottom quad
	PointBuffer[0] = cPoint3d(extent.x, extent.y, extent.z);//xNegyNegzNeg
	PointBuffer[1] = cPoint3d(extent.x, extent.y, extent.z);//xNegyNegzPos
	PointBuffer[2] = cPoint3d(extent.x, extent.y, extent.z);//xPosyNegzPos
	PointBuffer[3] = cPoint3d(extent.x, extent.y, extent.z);//xPosyNegzNeg

	//top quad
	PointBuffer[4] = cPoint3d(extent.x, extent.y, extent.z);//xNegyPoszNeg
	PointBuffer[5] = cPoint3d(extent.x, extent.y, extent.z);//xNegyPoszPos
	PointBuffer[6] = cPoint3d(extent.x, extent.y, extent.z);//xPosyPoszPos
	PointBuffer[7] = cPoint3d(extent.x, extent.y, extent.z);//xPosyPoszNeg
}


void emath::cOrientedBox::GetPoints(emath::cPoint3d PointBuffer[8]) const
{
	cMatrix mat = (zRot * xRot * yRot).GetMatrix();

	for (int i = 0; i < 8; i++)
		PointBuffer[i] = points[i] * mat + pos;
}

void emath::cOrientedBox::GetNormals(emath::cPoint3d norms[6]) const
{
	cPoint3d TransPoints[8];

	GetPoints(TransPoints);

	norms[0] = (TransPoints[1] - TransPoints[0]).cross(TransPoints[2] - TransPoints[1]).normalize();//yPos
	norms[1] = (TransPoints[7] - TransPoints[4]).cross(TransPoints[6] - TransPoints[7]).normalize();//yNeg

	norms[2] = (TransPoints[3] - TransPoints[2]).cross(TransPoints[6] - TransPoints[3]).normalize();//xPos
	norms[3] = (TransPoints[1] - TransPoints[0]).cross(TransPoints[4] - TransPoints[1]).normalize();//xNeg

	norms[4] = (TransPoints[2] - TransPoints[1]).cross(TransPoints[5] - TransPoints[2]).normalize();//zPos
	norms[5] = (TransPoints[4] - TransPoints[0]).cross(TransPoints[7] - TransPoints[4]).normalize();//zNeg
}


emath::cOrientedBox emath::cOrientedBox::CombineBoxes(const cOrientedBox& box1, const cOrientedBox& box2)
{
	float xMax, xMin, yMax, yMin, zMax, zMin;

	cPoint3d b1points[8];
	cPoint3d b2points[8];
	cPoint3d RetPoints[8];

	box1.GetPoints(b1points);
	box2.GetPoints(b2points);


	//calculate maxes and mins
	xMax = xMin = b1points[0].x;
	yMax = yMin = b1points[0].y;
	zMax = zMin = b1points[0].z;

	for (int i = 0; i < 8; i++)
	{
		cPoint3d buf1 = b1points[i];
		cPoint3d buf2 = b2points[i];

		xMax = max<float>(xMax, max<float>(buf1.x, buf2.x));
		xMin = min<float>(xMin, min<float>(buf1.x, buf2.x));

		yMax = max<float>(yMax, max<float>(buf1.y, buf2.y));
		yMin = min<float>(yMin, min<float>(buf1.y, buf2.y));

		zMax = max<float>(zMax, max<float>(buf1.z, buf2.z));
		zMin = min<float>(zMin, min<float>(buf1.z, buf2.z));
	}

	//great, now we fill out RetPoints with all of the vertices of the new box
	
	//z minimum face
	RetPoints[0] = cPoint3d(xMin, yMin, zMin);
	RetPoints[1] = cPoint3d(xMin, yMax, zMin);
	RetPoints[2] = cPoint3d(xMax, yMax, zMin);
	RetPoints[3] = cPoint3d(xMax, yMin, zMin);

	//z maximum face
	RetPoints[4] = cPoint3d(xMin, yMin, zMax);
	RetPoints[5] = cPoint3d(xMin, yMax, zMax);
	RetPoints[6] = cPoint3d(xMax, yMax, zMax);
	RetPoints[7] = cPoint3d(xMax, yMin, zMax);

	return cOrientedBox(RetPoints, cPoint3d(0.0f, 0.0f, 0.0f));
}

bool emath::cOrientedBox::intersects(const cOrientedBox& other) const
{
	cPoint3d MyPoints[8];
	cPoint3d OtherPoints[8];

	cPoint3d MyVecs[6];
	cPoint3d OtherVecs[6];

	float MyMin, MyMax, OtherMin, OtherMax;

	MyMin = MyMax = OtherMin = OtherMax = 0.0f;


	GetPoints(MyPoints);
	other.GetPoints(OtherPoints);

	GetNormals(MyVecs);
	other.GetNormals(OtherVecs);


	//okay, we start by testing the x axis
	cPoint3d xAxis(1.0f, 0.0f, 0.0f);

	MyMin = MyMax = MyPoints[0].dot(xAxis);
	OtherMin = OtherMax = OtherPoints[0].dot(xAxis);

	for (int i = 0; i < 8; i++)
	{
		float TestDot = MyPoints[i].dot(xAxis);
		if (MyMin > TestDot)
			MyMin = TestDot;
		else if (MyMax < TestDot)
			MyMax = TestDot;

		TestDot = OtherPoints[i].dot(xAxis);

		if (OtherMin > TestDot)
			OtherMin = TestDot;
		else if (OtherMax < TestDot)
			OtherMax = TestDot;
	}

//	printf("OtherMin is %f, OtherMax is %f, MyMin is %f, and MyMax is %f\n", OtherMin, OtherMax, MyMin, MyMax);

	if (!(OtherMin <= MyMax && OtherMax >= MyMin))
		return false;//the x axis is a separating axis


	//now we check the y axis
	cPoint3d yAxis(0.0f, 1.0f, 0.0f);

	MyMin = MyMax = MyPoints[0].dot(yAxis);
	OtherMin = OtherMax = OtherPoints[0].dot(yAxis);

	for (int i = 0; i < 8; i++)
	{
		float TestDot = MyPoints[i].dot(yAxis);
		if (MyMin > TestDot)
			MyMin = TestDot;
		else if (MyMax < TestDot)
			MyMax = TestDot;

		TestDot = OtherPoints[i].dot(yAxis);

		if (OtherMin > TestDot)
			OtherMin = TestDot;
		else if (OtherMax < TestDot)
			OtherMax = TestDot;
	}

//	printf("OtherMin is %f, OtherMax is %f, MyMin is %f, and MyMax is %f\n", OtherMin, OtherMax, MyMin, MyMax);

	if (!(OtherMin <= MyMax && OtherMax >= MyMin))
		return false;//the y axis is a separating axis


	//and now for the z axis
	cPoint3d zAxis(0.0f, 0.0f, 1.0f);

	MyMin = MyMax = MyPoints[0].dot(zAxis);
	OtherMin = OtherMax = OtherPoints[0].dot(zAxis);

	MyMin = MyMax = OtherMin = OtherMax = 0.0f;

	for (int i = 0; i < 8; i++)
	{
		float TestDot = MyPoints[i].dot(zAxis);
		if (MyMin > TestDot)
			MyMin = TestDot;
		else if (MyMax < TestDot)
			MyMax = TestDot;

		TestDot = OtherPoints[i].dot(zAxis);

		if (OtherMin > TestDot)
			OtherMin = TestDot;
		else if (OtherMax < TestDot)
			OtherMax = TestDot;
	}

//	printf("OtherMin is %f, OtherMax is %f, MyMin is %f, and MyMax is %f\n", OtherMin, OtherMax, MyMin, MyMax);

	if (!(OtherMin <= MyMax && OtherMax >= MyMin))
		return false;//the z axis is a separating axis





	//okay, now we check every normal of both boxes to see if any of them are separating axes
	for (int j = 0; j < 6; j++)
	{
		cPoint3d *axis = &MyVecs[j];

		MyMin = MyMax = MyPoints[0].dot(*axis);
		OtherMin = OtherMax = OtherPoints[0].dot(*axis);

		for (int i = 0; i < 8; i++)
		{
			float TestDot = MyPoints[i].dot(*axis);
			if (MyMin > TestDot)
				MyMin = TestDot;
			else if (MyMax < TestDot)
				MyMax = TestDot;

			TestDot = OtherPoints[i].dot(*axis);

			if (OtherMin > TestDot)
				OtherMin = TestDot;
			else if (OtherMax < TestDot)
				OtherMax = TestDot;
		}

//		printf("OtherMin is %f, OtherMax is %f, MyMin is %f, and MyMax is %f\n", OtherMin, OtherMax, MyMin, MyMax);

		if (!(OtherMin <= MyMax && OtherMax >= MyMin))
			return false;//axis is a separating axis


	

		//okay, now we check the corresponding normal in the other box

		axis = &OtherVecs[j];

		MyMin = MyMax = MyPoints[0].dot(*axis);
		OtherMin = OtherMax = OtherPoints[0].dot(*axis);

		for (int i = 0; i < 8; i++)
		{
			float TestDot = MyPoints[i].dot(*axis);
			if (MyMin > TestDot)
				MyMin = TestDot;
			else if (MyMax < TestDot)
				MyMax = TestDot;

			TestDot = OtherPoints[i].dot(*axis);

			if (OtherMin > TestDot)
				OtherMin = TestDot;
			else if (OtherMax < TestDot)
				OtherMax = TestDot;
		}

//		printf("OtherMin is %f, OtherMax is %f, MyMin is %f, and MyMax is %f\n", OtherMin, OtherMax, MyMin, MyMax);

		if (!(OtherMin <= MyMax && OtherMax >= MyMin))
			return false;//axis is a separating axis
	}


	return true;//collision
}
